﻿using Messages.roscpp_tutorials;
using RosSharp;
using System;
using System.Threading;

namespace ServiceClient
{
    class Program
    {
        static void Main(string[] args)
        {
            ROS.ROS_MASTER_URI = "http://192.168.20.132:11311";
            ROS.ROS_HOSTNAME = "192.168.20.1";
            ROS.ROS_IP = "192.168.20.132";
            ROS.Init("ServiceClientTest");
            NodeHandle nodeHandle = new NodeHandle();
            new Thread(new ThreadStart(() =>
            {
                Random r = new Random();
                while (ROS.ok)
                {
                    try
                    {
                        TwoInts.Request req = new TwoInts.Request() { a = r.Next(100), b = r.Next(100) };
                        TwoInts.Response resp = new TwoInts.Response();
                        DateTime before = DateTime.Now;
                        bool res = nodeHandle.ServiceClient<TwoInts.Request, TwoInts.Response>("/add_two_ints").Call(req, ref resp);
                        TimeSpan dif = DateTime.Now.Subtract(before);

                        string str = "";
                        if (res)
                            str = "" + req.a + " + " + req.b + " = " + resp.sum + "\n";
                        else
                            str = "call failed after\n";
                        str += Math.Round(dif.TotalMilliseconds, 2) + " ms";
                        Console.WriteLine(str);
                    }
                    catch
                    {

                    }
                    Thread.Sleep(500);
                }
            })).Start();
            Console.CancelKeyPress += Console_CancelKeyPress;
        }

        private static void Console_CancelKeyPress(object sender, ConsoleCancelEventArgs e)
        {
            ROS.Shutdown();
        }
    }
}
